// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SDFTools.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Geometry.SDF
{    
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Class that implements various tools for a signed distance function
    /// </summary>
    public static class SDFTools
    {
        /// <summary>
        /// Computes a signed distance function from a depth image
        /// </summary>
        /// <param name="sdf">The signed distance function to populate</param>
        /// <param name="depth">The depth image</param>
        /// <param name="width">Width of the depth image</param>
        /// <param name="height">Height of the depth image</param>
        /// <param name="pose">The pose of the depth camera</param>
        /// <param name="intrinsics">The camera $intrinsics$</param>
        public static void DepthToSDF(SDF sdf, short[] depth, int width, int height, Pose pose, Matrix3 intrinsics)
        {
            CoordinateTransform3D transform = new CoordinateTransform3D(pose).Reverse();
            Matrix3 inverseIntrinsics = intrinsics.Inverse();
           
            Parallel.For(
                0,
                sdf.Depth,
                (slice) =>
                {
                    for (int row = 0; row < sdf.Height; row++)
                    {
                        for (int col = 0; col < sdf.Width; col++)
                        {
                            // get the 3D point of the grid
                            Vector3 point3d = sdf.Get3DPoint(col, row, slice);

                            // warp point to camera coordinate frame
                            point3d = transform.Transform(point3d);

                            // point is behind the camera
                            if (point3d.Z < 0)
                            {
                                sdf[col, row, slice] = double.NaN;
                                continue;
                            }

                            // project onto the depth image plane
                            double x = point3d.X * intrinsics[0, 0] / point3d.Z + intrinsics[0, 2];
                            double y = point3d.Y * intrinsics[1, 1] / point3d.Z + intrinsics[1, 2];

                            // projected image lies in the depth image
                            if (x >= 0 && x < width && y >= 0 && y < height)
                            {
                                // get the index into the depth image
                                int index = (int)y * width + (int)x;

                                if (depth[index] != 0)
                                {
                                    // Compute scalar
                                    double lambda = (inverseIntrinsics * new Vector3(x, y, 1)).Length();

                                    // Compute signed distance
                                    sdf[col, row, slice] = (point3d).Length() / lambda - (double)depth[index] / 1000.0f;
                                }
                                else
                                {
                                    sdf[col, row, slice] = double.NaN;
                                }
                            }
                            else
                            {
                                sdf[col, row, slice] = double.NaN;
                            }
                        }
                    }
            });
        }
    }
}
